#!/usr/bin/env python
import rospy
from aikit_arm.srv import aikit_claw_srv
from std_msgs.msg import Int32
rospy.wait_for_service("aikit_claw")
claw = rospy.ServiceProxy("aikit_claw", aikit_claw_srv)
rospy.init_node("claw_speed",anonymous=True)
claw_s = 50

claw_sleep = 0

def cb_claw(msg):
    global claw_sleep
    if msg.data == 0:
        claw_sleep = 0
    else:
        claw_sleep = 1.0/float(msg.data)
def getspeed():
    rospy.Subscriber("claw_control", Int32, cb_claw)

def set_claw_speed():
    global claw_s
    if claw_sleep > 0:
        claw_s -= 1
    elif claw_sleep < 0:
        claw_s += 1


    if claw_s>100:
        claw_s=100
    if claw_s<0:
        claw_s=0

    claw(claw_s)
    rospy.sleep(abs(claw_sleep))

if __name__ == '__main__':
	try:
		getspeed()
		while not rospy.is_shutdown():
		    set_claw_speed()
		rospy.spin()
	except:
		pass
